#include <bits/stdint-intn.h>
#include "mg_typedef.h"
#include "message_loop/message_loop.h"
#include "obstacle_avoidance_sonar.h"
#if defined( __linux__ )
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <netinet/udp.h>
#elif defined( __VXWORKS__ )
#endif
static struct SonarAttr {
  int32_t            socket_fd_;
  struct sockaddr_in sock_addr_;
  struct ev_loop*    loop_;
} g_sonar_attri;
static void sonar_message_handler( EV_P_ ev_timer* w, int revents ) {
  int32_t             lengh = 0;
  struct SonarAttr*   attr  = ( struct SonarAttr* )w->data;
  SonarControlCommand buf;
  memset( &buf, 1, sizeof( buf ) );
  lengh = sendto( attr->socket_fd_,
                  &buf,
                  sizeof( buf ),
                  0,
                  ( const struct sockaddr* )&attr->sock_addr_,
                  sizeof( struct sockaddr_in ) );
  MG_DEBUG( "send data length=%d", lengh );
  //
  w->repeat = 5;
  ev_timer_again( attr->loop_, w );
}
void sonar_test_tool_init( ) {
  int32_t socket_fd                        = -1;
  int32_t ret                              = -1;

  g_sonar_attri.sock_addr_.sin_family      = AF_INET;
  g_sonar_attri.sock_addr_.sin_addr.s_addr = inet_addr( "127.0.0.1" );
  g_sonar_attri.sock_addr_.sin_port        = htons( 2222 );

  //
  socket_fd = socket( AF_INET, SOCK_DGRAM, IPPROTO_UDP );

  {
    static ev_timer        timeout_watcher;
    static struct ev_loop* loop = NULL;
    if ( !loop ) {
      loop = ev_loop_new( 0 );
    }
    ev_timer_init( &timeout_watcher, sonar_message_handler, 2, 0. );
    g_sonar_attri.loop_      = loop;
    g_sonar_attri.socket_fd_ = socket_fd;
    timeout_watcher.data     = &g_sonar_attri;
    ev_timer_start( loop, &timeout_watcher );
    ev_run( loop, 0 );
  }
}